

#ifndef __LOOP_PLANNER_H__
#define __LOOP_PLANNER_H__

#include "cell_decomposer.h"
#include "preprocess/map_process.h"
#include "common/vec2d.h"
#include "path_smooth/path_smoother.h"

using namespace global_path_planner::common;
using namespace global_path_planner::preprocess;
using namespace global_path_planner::path_smooth;

// #include <opencv2/opencv.hpp>

namespace global_path_planner {
namespace ccpp {

class LoopPlanner{
public:
    explicit LoopPlanner(std::shared_ptr<MapProcess>& map_process,
                        std::shared_ptr<CellDecomposer>& cell_decomp,
                        std::shared_ptr<Polygon2D>& poly2d);
    virtual ~LoopPlanner();

    // 回字形路径
    std::unordered_map<int, std::vector<std::vector<cv::Point>>>
    getLoopPaths(Point_2 &start, cv::Mat img, float& sweep_interval,
                float& sweep_width, float& sweep_offset,
                std::vector<std::vector<cv::Point>> polys);

    // 贴边路径
    std::vector<std::vector<cv::Point>>
    getPerimPaths(cv::Mat& img, int& narrow_lane,
                float& sweep_width, float& sweep_interval,
                std::vector<std::vector<cv::Point>> all_polygons);

    // 获取回字形/贴边
    std::vector<std::vector<cv::Point>> getWrapperPath(cv::Mat &img, int perim, float &sweep_width,
                                                    float &sweep_interval, float &sweep_offset,
                                                    bool& contain_circle, bool& loop_smooth,
                                                    std::vector<std::vector<cv::Point>> &polys);

    // 根据起点对所有回字形进行排序
    std::vector<std::vector<cv::Point>>
    sortAllLoopPaths(Point_2 &start,
                    std::unordered_map<int, std::vector<std::vector<cv::Point>>> origin_loop_paths);

    // 回字形/贴边排序
    std::vector<std::vector<cv::Point>>
    sortLoopPaths(cv::Point& start,
        std::vector<std::vector<cv::Point>> loop_paths);

private:
    // 优化轨迹
    std::vector<std::vector<cv::Point>>
    getOptimizedPaths(int perim, cv::Mat &img,
        std::vector<std::vector<cv::Point>> pts);

    // 路径增加致密点
    std::vector<std::vector<cv::Point>> 
    getDensePoints(int perim, PathSmoother& path_smoother, 
                std::vector<std::vector<cv::Point>> sparsePaths);

private:
    std::shared_ptr<CellDecomposer> __cell_Decomp_Ptr;
    std::shared_ptr<MapProcess> __map_Proc_Ptr;
    std::shared_ptr<Polygon2D> __polygon2d_Ptr;
    // common::CoordinateConversion __coord_Conversion;

};

}  // namespace ccpp
}  // namespace global_path_planner

#endif  // __LOOP_PLANNER_H__